﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Woodstock.Common;
using System.ComponentModel;
using Woodstock.HAL.Phidgets;
using System.Threading;
using System.Collections.ObjectModel;
using System.Windows.Threading;

namespace Woodstock.Quad
{
    public enum QuadState
    { 
        Stable, // Craft stable
        Recovery, // Craft have more than 45 degrees on any axis. Recovery program must be executed.
        Cruise, // Craft is in cruise state, angulation is OK and executed because of pilot commands
        Command // Craft is executing a programmed command.
    }


    public class QuadFlightControl : INotifyPropertyChanged
    {
        private bool _on;
        public bool On
        {
            get { return _on; }
            set 
            {
                _on = value; 
                NotifyPropertyChanged("On");
            }
        }



        Queue<SpatialState> SpatialStateHistory;

        
        // Defines how agressive the gyro auto control is.
        public double Gain { get; set; }

        public Motor Motor1 { get; private set; }
        public Motor Motor2 { get; private set; }
        public Motor Motor3 { get; private set; }
        public Motor Motor4 { get; private set; }

        // Trims are calculated dynamically.
        // It's intention is to have small variance when the craft is close to its target angle - just to trim it and make sure it's stable
        public double Motor1Trim { get; private set; }
        public double Motor2Trim { get; private set; }
        public double Motor3Trim { get; private set; }
        public double Motor4Trim { get; private set; }

        // TODO: Recovery Trims
        // Are intended to recover the craft quickly when it's too off its target angle. 
        // Maximum power might be used here until the craft enters its target angle trim zone




        // Defines the global motor speed, or current trottle of the quad. Set by the pilot and no motor can go below this value
        private double _globalMotorSpeed;
        public double GlobalMotorSpeed
        {
            get { return _globalMotorSpeed; }
            set
            {
                _globalMotorSpeed = value;
                NotifyPropertyChanged("GlobalMotorSpeed");
            }
        }



        public double TargetRollAngle { get; set; }
        public double TargetPitchAngle { get; set; }

        
        public Command CurrentCommand { get; set; }


        private SpatialState _currentSpatialState;
        public SpatialState CurrentSpatialState
        {
            get { return _currentSpatialState; }
            set { _currentSpatialState = value; NotifyPropertyChanged("CurrentSpatialState"); }
        }




        





        public event PropertyChangedEventHandler PropertyChanged;

        private void NotifyPropertyChanged(String info)
        {
            if (PropertyChanged != null)
            {
                PropertyChanged(this, new PropertyChangedEventArgs(info));
            }
        }



        public ISpatialStateProvider SpatialStateProvider { get; set; }


        public QuadFlightControl(ISpatialStateProvider stateProvider, Motor motor1, Motor motor2, Motor motor3, Motor motor4)
        {
            SpatialStateProvider = stateProvider;
            Motor1 = motor1;
            Motor2 = motor2;
            Motor3 = motor3;
            Motor4 = motor4;

            // Create a new spatial state history
            SpatialStateHistory = new Queue<SpatialState>();
            
            // Subscribe to state changes
            SpatialStateProvider.SpatialStateChanged += new EventHandler<SpatialStateEventArgs>(SpatialStateProvider_SpatialStateChanged);            

            // Set default gain
            Gain = 1;
        }

        void SpatialStateProvider_SpatialStateChanged(object sender, SpatialStateEventArgs e)
        {
            // We have a new spatial state. We can make decisions now.
            CurrentSpatialState = e.State;

            
            //
            // To keep it stable, aceleration data must be 0 and gyro data stable (regardless of angle value)
            //


            if (On)
            {
                // Pitch angle = Motors 1(+) and 3(-)
                // Roll angle = Motors 4(+) and 2(-)


                #region Calculate pitch difference
                {

                    // Check the difference from the current angle to the target angle
                    double pitchDifference = CurrentSpatialState.Pitch - TargetPitchAngle;

                    // Get a positive difference
                    if (pitchDifference < 0)
                    {
                        pitchDifference = pitchDifference * -1;
                    }

                    // Calculate the ammount of power we shift depending on the angle
                    // If we have a 90 degree change, we add 50% * gain of power.

                    double pitchPowerChange = pitchDifference / 90;
                    pitchPowerChange = pitchPowerChange * Gain;

                    //
                    // Tilting on Pitch axis
                    //
                    if (CurrentSpatialState.Pitch > TargetPitchAngle)
                    {
                        // Pitch if more than target angle. We shift back

                        // First, see if we can drop trim power on oposite engine
                        if (Motor3Trim > 0)
                        {
                            if (Motor3Trim >= pitchPowerChange)
                            {
                                // Just reduce power on oposite motor
                                Motor3Trim -= pitchPowerChange;
                                pitchPowerChange = 0;
                            }
                            else
                            {
                                // We drop as much as we can
                                pitchPowerChange -= Motor3Trim;
                                Motor3Trim = 0;
                            }
                        }

                        // Then, see if we still need to apply power on engine
                        if (pitchPowerChange > 0)
                        {
                            // Increase power on this angle engine
                            Motor1Trim += pitchPowerChange;
                        }

                        // Check we're not left with a negative value
                        if (Motor1Trim < 0)
                            Motor1Trim = 0;
                    }
                    else
                    {
                        // Pitch if less than target angle. We shift forwards

                        // First, see if we can drop trim power on oposite engine
                        if (Motor1Trim > 0)
                        {
                            if (Motor1Trim >= pitchPowerChange)
                            {
                                // Just reduce power on oposite motor
                                Motor1Trim -= pitchPowerChange;
                                pitchPowerChange = 0;
                            }
                            else
                            {
                                // We drop as much as we can
                                pitchPowerChange -= Motor3Trim;
                                Motor1Trim = 0;
                            }
                        }

                        // Then, see if we still need to apply power on engine
                        if (pitchPowerChange > 0)
                        {
                            // Increase power on this angle engine
                            Motor3Trim += pitchPowerChange;
                        }

                        // Check we're not left with a negative value
                        if (Motor3Trim < 0)
                            Motor3Trim = 0;
                    }
                }
                #endregion





                //
                // Calculate roll difference
                //

                // Check the difference from the current angle to the target angle
                double rollDifference = CurrentSpatialState.Pitch - TargetPitchAngle;

                // Get a positive difference
                if (rollDifference < 0)
                {
                    rollDifference = rollDifference * -1;
                }

                // Calculate the ammount of power we shift depending on the angle
                // If we have a 90 degree change, we add 50% * gain of power.

                double rollPowerChange = rollDifference / 90;
                rollPowerChange = rollPowerChange * Gain;


















                /*// Check if this been happening for the past 10 measurements
                    var pastMeasurements = from x in SpatialStateHistory.Take(10)
                                           select x.XAcceleration;

                    bool constantInPast10Measurements = true;
                    foreach (var measure in pastMeasurements)
                    {
                        if (measure < 0)
                        {
                            constantInPast10Measurements = false;
                            break;
                        }
                    }

                    //if (constantInPast10Measurements)
                    {*/
                // This is a constant behaviour. Let's trim that motor for stability.
                // If this situation is not constant, we're probably just drifting from vibration. No action needed



                // First, let's check our acceleration vector. If it's positive on that axis for the past 10 measurements, we get the average vector in the same past time to make a decision

                // Tilting on Pitch axis
                if (CurrentSpatialState.Pitch > 0)
                {
                    // We're dropping on motor 1 angle

                    // Let's check our current angular velocity to see how fast we're dropping

                    double angularChangeTrimGain = 0;
                    angularChangeTrimGain = e.State.XAngle / 90;


                    // First, see if we can drop trim power on oposite engine
                    if (Motor3Trim > 0)
                    {
                        if (Motor3Trim >= angularChangeTrimGain)
                        {
                            // Just reduce power on oposite motor
                            Motor3Trim -= angularChangeTrimGain;
                            angularChangeTrimGain = 0;
                        }
                        else
                        {
                            // We drop as much as we can
                            angularChangeTrimGain -= Motor3Trim;
                            Motor3Trim = 0;
                        }
                    }

                    // Then, see if we still need to apply power on engine
                    if (angularChangeTrimGain > 0)
                    {
                        // Increase power on this angle engine
                        Motor1Trim += angularChangeTrimGain;
                    }

                    // Check we're not left with a negative value
                    if (Motor1Trim < 0)
                        Motor1Trim = 0;
                }
                else
                {
                    // We're dropping on motor 3 angle

                    double angularChangeTrimGain = 0;
                    angularChangeTrimGain = (e.State.XAngle * -1) / 90;


                    // First, see if we can drop trim power on oposite engine
                    if (Motor1Trim > 0)
                    {
                        if (Motor1Trim >= angularChangeTrimGain)
                        {
                            // Just reduce power on oposite motor
                            Motor1Trim -= angularChangeTrimGain;
                            angularChangeTrimGain = 0;
                        }
                        else
                        {
                            // We drop as much as we can
                            angularChangeTrimGain -= Motor1Trim;
                            Motor1Trim = 0;
                        }
                    }

                    // Then, see if we still need to apply power on engine
                    if (angularChangeTrimGain > 0)
                    {
                        // Increase power on this angle engine
                        Motor3Trim += angularChangeTrimGain;
                    }

                    // Check we're not left with a negative value
                    if (Motor3Trim < 0)
                        Motor3Trim = 0;
                }

                Motor1.Position = GlobalMotorSpeed + Motor1Trim;
                Motor3.Position = GlobalMotorSpeed + Motor3Trim;




                if (CurrentSpatialState.XAngle > 0)
                {
                    //
                    // We're going foward on Y axis
                    //

                    double angleMotorSpeedChange = 0;
                    angleMotorSpeedChange = 100 * (CurrentSpatialState.XAngle / 90);

                    /*Motor1.Position = GlobalMotorSpeed - angleMotorSpeedChange;
                    Motor4.Position = GlobalMotorSpeed - angleMotorSpeedChange;

                    Motor2.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    Motor3.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    */

                    // If we're over 90 degrees apply max speed (1 or more)
                }
                else if (CurrentSpatialState.XAngle < 0)
                {
                    //
                    // We're going backwards on Y axis
                    //

                    double angleMotorSpeedChange = 0;
                    angleMotorSpeedChange = 100 * ((CurrentSpatialState.XAngle * -1) / 90);

                    /*
                    Motor2.Position = GlobalMotorSpeed - angleMotorSpeedChange;
                    Motor3.Position = GlobalMotorSpeed - angleMotorSpeedChange;

                    Motor1.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    Motor4.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    */
                }


                if (CurrentSpatialState.YAngle > 0)
                {
                    //
                    // We're going foward on X axis, increase speed on motors 1 and 4, decrease on 2 and 3
                    //

                    double angleMotorSpeedChange = 0;
                    angleMotorSpeedChange = 100 * (CurrentSpatialState.YAngle / 90);

                    /*
                    Motor1.Position = GlobalMotorSpeed - angleMotorSpeedChange;
                    Motor2.Position = GlobalMotorSpeed - angleMotorSpeedChange;

                    Motor3.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    Motor4.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    */
                }
                else if (CurrentSpatialState.YAngle < 0)
                {
                    //
                    // We're going backwards on X axis, increase speed on motors 2 and 3, decrease on 1 and 4
                    //

                    double angleMotorSpeedChange = 0;
                    angleMotorSpeedChange = 100 * ((CurrentSpatialState.YAngle * -1) / 90);

                    /*
                    Motor3.Position = GlobalMotorSpeed - angleMotorSpeedChange;
                    Motor4.Position = GlobalMotorSpeed - angleMotorSpeedChange;

                    Motor1.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    Motor2.Position = GlobalMotorSpeed + angleMotorSpeedChange;
                    */
                }
            }


            // Register this spatial state as history
            SpatialStateHistory.Enqueue(CurrentSpatialState);

            if (SpatialStateHistory.Count > 100)
            {
                // Remove the oldest state
                SpatialStateHistory.Dequeue();
            }
        }        
    }
}
